/* 
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;

/**
 *
 * @author matt
 */
public class Lifter implements MotorSafety {

    protected MotorSafetyHelper m_safetyHelper;
//    Relay lifterRDrive = null;
    Victor lifterVDrive = null;
    AnalogChannel lifterPosition = null;
    PIDController lifterControl = null;
    private boolean m_isStarted = false;
    private int m_commandedPosition;

    public Lifter(int drive, int sense) {
        System.out.println("Lifter Constructor (" + drive + "," + sense + ")");
        lifterVDrive = new Victor(drive);
        lifterPosition = new AnalogChannel(sense);

        double gain_Kp = 0.05; // Proportional
        double gain_Ki = 0.00; // Integral
        double gain_Kd = 0.0; // Derivative

        lifterControl = new PIDController(gain_Kp, gain_Ki, gain_Kd, lifterPosition, lifterVDrive);
        lifterControl.setContinuous(false);
        lifterControl.setInputRange((double) 930, (double) 980);
        lifterControl.setOutputRange(-1.0, 1.0);
        lifterControl.setTolerance(2);  // percent        

        setupMotorSafety();

        //setPosition(970);
    }

    public void startLifter() {
        // wrapped to prevent calling synchronized code in PID class unnecessarily
        if (!m_isStarted) {
            lifterControl.enable();
            m_isStarted = true;
            System.out.println("Lifter Start");
        }
    }

    public void stopLifter() {
        // wrapped to prevent calling synchronized code in PID class unnecessarily
        if (m_isStarted) {
//             lifterVDrive.set(0);
            lifterControl.disable();
            m_isStarted = false;
        }
    }

    public void setPosition(int count) {
//        System.out.println("Lifter setPosition");
//        m_commandedPosition = count;
        lifterControl.setSetpoint((double) count);
//        lifterControl.reset();
    }

    public double getCommandedPosition() {
        return lifterControl.getSetpoint();
    }

    public double getPosition() {
        if (null != lifterPosition) {
            return lifterPosition.pidGet();
        } else {
            return 0;
        }
    }

    public double getControlImpulse() {
        return lifterControl.get();
    }

    public double getError() {
        return lifterControl.getError();
    }

    public boolean isInPosition() {
        return lifterControl.onTarget();

    }
    // WPI/FIRST MOTOR SAFETY GARBAGE
    public static final double kDefaultExpirationTime = 10.0;

    public String getDescription() {
        return "Lifter Drive";
    }

    public void stopMotor() {
        if (lifterVDrive != null) {
            lifterVDrive.set(0.0);
        }
    }

    private void setupMotorSafety() {
        m_safetyHelper = new MotorSafetyHelper(this);
        m_safetyHelper.setExpiration(kDefaultExpirationTime);
        m_safetyHelper.setSafetyEnabled(false);
    }

    public boolean isSafetyEnabled() {
        return m_safetyHelper.isSafetyEnabled();
    }

    public void setSafetyEnabled(boolean enabled) {
        m_safetyHelper.setSafetyEnabled(enabled);
    }

    public void setExpiration(double timeout) {
        m_safetyHelper.setExpiration(timeout);
    }

    public double getExpiration() {
        return m_safetyHelper.getExpiration();
    }

    public boolean isAlive() {
        return m_safetyHelper.isAlive();
    }
}
